#include <dh_hand_driver/ag95_grasp.h>


// Called once when the goal completes
void DH_HandActionClient::DoneCb(
    const actionlib::SimpleClientGoalState &state,
    const dh_hand_driver::ActuateHandResultConstPtr &result) {
  ROS_INFO("Finished in state [%s]", state.toString().c_str());
  ROS_INFO("result  : %i", result->opration_done);
}

// when target active, call this once
void DH_HandActionClient::ActiveCb() { ROS_INFO("Goal just went active"); }

// received feedback
void DH_HandActionClient::FeedbackCb(
    const dh_hand_driver::ActuateHandFeedbackConstPtr &feedback) {
  ROS_INFO("Got Feedback: %i", feedback->position_reached);
}

DH_HandActionClient::DH_HandActionClient(const std::string client_name,
                                         bool flag = true)
    : client(client_name, flag) {}

// client start
void DH_HandActionClient::Start(int32_t motorID, int32_t setpos,
                                int32_t setforce) {
  ROS_INFO("wait server");
  client.waitForServer();
  // set goal
  dh_hand_driver::ActuateHandGoal goal;
  // AG2E just has one motor (ID:1)
  // AG3E has two motor (ID:1 and 2)
  goal.MotorID = motorID;
  goal.force = setforce;
  goal.position = setpos;

  ROS_INFO("Send goal");
  // sen goal
  client.sendGoal(goal, boost::bind(&DH_HandActionClient::DoneCb, this, _1, _2),
                  boost::bind(&DH_HandActionClient::ActiveCb, this),
                  boost::bind(&DH_HandActionClient::FeedbackCb, this, _1));
  ROS_INFO("wait result");

  client.waitForResult(ros::Duration(10.0));

  // process the result
  if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    ROS_INFO("Send commond succeeded");
  else {
    ROS_INFO("Cancel Goal!");
    client.cancelAllGoals();
  }

  ROS_INFO("Current State: %s\n", client.getState().toString().c_str());
}

bool AG95_grasp(ros::NodeHandle nh_,int32_t motorID,int32_t setpos,int32_t setforce) {
  ROS_INFO("starting");
  DH_HandActionClient actionclient("actuate_hand", true);
  ROS_INFO("starting client");
  actionclient.Start(motorID, setpos, setforce);

  ros::spinOnce();

  // use service to get hand state
  ros::ServiceClient client =
      nh_.serviceClient<dh_hand_driver::hand_state>("hand_joint_state");
  dh_hand_driver::hand_state srv;
  srv.request.get_target = 0;
  if (client.call(srv)) {
    ROS_INFO("force: %d", srv.response.return_data);
  } else {
    ROS_ERROR("Failed to call service");
    return 1;
  }
  srv.request.get_target = 1;
  if (client.call(srv)) {
    ROS_INFO("pos_1: %d", srv.response.return_data);
  } else {
    ROS_ERROR("Failed to call service");
    return 1;
  }
  return 0;
}

